#include "match_center.h"
namespace perception
{
    namespace algorithm
    {
        static inline double normalize_angle_positive(double angle)
        {
            // Normalizes the angle to be 0 to 2*M_PI.
            // It takes and returns radians.
            return fmod(fmod(angle, 2.0 * M_PI) + 2.0 * M_PI, 2.0 * M_PI);
        }
        static inline double normalize_angle(double angle)
        {
            // Normalizes the angle to be -M_PI circle to +M_PI circle
            // It takes and returns radians.
            double a = normalize_angle_positive(angle);
            if (a > M_PI)
                a -= 2.0 * M_PI;
            return a;
        }

        static inline double shortest_angular_distance(double from, double to)
        {
            // Given 2 angles, this returns the shortest angular difference.
            // The inputs and outputs are radians.
            // The result would always be -pi <= result <= pi.
            // Adding the result to "from" results to "to".
            return normalize_angle(to - from);
        }
        MatchCenter::MatchCenter(track_config &config)
        {
            this->max_confidence = config.max_confidence;
            this->dconfidence = config.dconfidence;
            this->now_v_ratio = config.now_v_ratio;
        }
        void MatchCenter::Init(unsigned long int id, LidarObject &new_points,
                               const double &time, const Localization &ego_pose)
        {
            int n = 6; // Number of states
            // int n = 4;        // Number of states
            int m = 2;        // Number of measurements
            MatrixXd A(n, n); // System dynamics matrix
            MatrixXd C(m, n); // Output matrix
            MatrixXd Q(n, n); // Process noise covariance
            MatrixXd R(m, m); // Measurement noise covariance
            MatrixXd P(n, n); // Estimate error covariance
            double dt = 0.1;
            double ddt = dt * dt / 2;
            A << 1, 0, dt, 0, ddt, 0, 0, 1, 0, dt, 0, ddt, 0, 0, 1, 0, dt, 0, 0, 0, 0, 1,
                0, dt, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;

            C << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0;

            Q << 0.1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.1,
                0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0.5;

            R.setIdentity();
            R *= 10;
            R *= 0.1;
            // P.setIdentity() * 0.1;
            P.setIdentity() * 1;

            KalmanFilterSimple dynamic_kalman_filter(dt, A, C, Q, R, P);
            this->dynamic_kf = dynamic_kalman_filter;

            VectorXd x0_dynamic(n);
            x0_dynamic << new_points.x, new_points.y, 0, 0, 0, 0;
            // x0_dynamic << x_corner, y_corner, 0, 0;
            dynamic_kf.init(0, x0_dynamic);

            this->id = id;
            this->age = 1;
            this->x = new_points.x;
            this->y = new_points.y;
            this->z = new_points.z;
            this->type = int(new_points.type);
            this->height_box = new_points.height;
            pre_obj = new_points;
            this->confidence = 3;

            this->psi = new_points.heading;
            this->length_box = new_points.length;
            this->width_box = new_points.width;
            this->cvx = 0;
            this->cvy = 0;
            this->ax = 0;
            this->ay = 0;
            CornerPoint p;
            p.x = this->x;
            p.y = this->y;
            all_points.emplace_back(p);
            pre_heading = new_points.heading;
            sum_x = sum_x + new_points.x;
            sum_y = sum_y + new_points.y;
            cp = new_points.corner_point;
            points_num = new_points.cluster_points.points.size();
            std::cout << "init id:" << id
                      << " x:" << this->x
                      << " y:" << this->y
                      << " this->cvx:" << this->cvx
                      << " this->cvy:" << this->cvy
                      << " heading:" << this->psi
                      << " confidence:" << this->confidence
                      << std::endl;
            last_time = time;
            // WARN("x:{},y:{},id:{}", this->x, this->y, this->id);
        }
        void MatchCenter::predict()
        {
            // dynamic_kf.predict();
            // this->x = dynamic_kf.state()(0);
            // this->y = dynamic_kf.state()(1);
        }
        void MatchCenter::update(LidarObject &new_points, const double &time,
                                 const Localization &ego_pose)
        {
            this->z = new_points.z;
            float old_x = this->x;
            float old_y = this->y;
            float new_x = new_points.x;
            float new_y = new_points.y;
            this->age++;
            CornerPoint center_p;
            center_p.x = new_x;
            center_p.y = new_y;
            all_points.emplace_back(center_p);
            if (all_points.size() > 10)
                all_points.erase(all_points.begin());
            // pre_obj = new_points;
            if (track_type_ == track_type::MATCHED)
                this->confidence = this->confidence + this->dconfidence;
            else if (track_type_ == track_type::MATCHING_FAILED)
                this->confidence = this->confidence - this->dconfidence;
            if (this->confidence > max_confidence)
                this->confidence = max_confidence;
            // std::cout << "new_points.x:" << new_points.x
            //           << " new_points.y:" << new_points.y
            //           << std::endl;

            Vector2d p;
            p << new_points.x, new_points.y;
            // dynamic_kf.update2(p);
            double dt = time - last_time;
            if (dt < 0)
                dt = 0.1;
            last_time = time;
            dynamic_kf.update(p, dt);
            auto x_tmp = this->x;
            auto y_tmp = this->y;
            auto vx_tmp = this->cvx;
            auto vy_tmp = this->cvy;
            // this->x = dynamic_kf.state()(0);
            // this->y = dynamic_kf.state()(1);

            this->cvx = dynamic_kf.state()(2);
            this->cvy = dynamic_kf.state()(3);
            // 认为是静止的
            if ((std::sqrt(this->cvx * this->cvx + this->cvy * this->cvy)) * 3.6 < 5)
            {
                // 取面积最多的作为航向
                // if (this->length_box * this->width_box < new_points.length * new_points.width)
                // 取点数最多
                if (pre_obj.cluster_points.size() < new_points.cluster_points.size())
                {
                    this->length_box = new_points.length;
                    this->width_box = new_points.width;
                    this->psi = new_points.heading;
                    this->type = new_points.type;
                    this->height_box = new_points.height;
                    pre_obj = new_points;
                    this->x = new_points.x;
                    this->y = new_points.y;
                    // TODO 中心点补偿 需要记录上一次的最近角点和朝向，然后根据当前最近角点判断是否需要转换，然后修正长宽及中心点
                }
            }
            else
            {
                this->x = new_points.x;
                this->y = new_points.y;
                // TODO:根据历史轨迹计算航向修正长宽高及中心点
                this->length_box = new_points.length;
                this->width_box = new_points.width;
                this->psi = new_points.heading;
                this->type = 0;
                this->height_box = new_points.height;
                pre_obj = new_points;
                auto tmp_all_points = all_points;
                // if (tmp_all_points.size() > 2)
                // {
                //     CoordinateTransformation(tmp_all_points, ego_pose);

                //     int degree = 2; // 多项式的次数
                //     MatrixXd X;
                //     VectorXd y_vec, beta;

                //     buildSystem(tmp_all_points, degree, X, y_vec);

                //     // 使用正常方程组解线性最小二乘问题
                //     beta = (X.transpose() * X).inverse() * X.transpose() * y_vec;

                //     // 输出多项式系数
                //     std::cout << "Polynomial coefficients: " << std::endl;
                //     std::cout << beta << std::endl;
                //     this->psi = beta[2] * this->x + beta[1] - ego_pose.angle * deg2rad;
                //     auto next_x = (tmp_all_points.end() - 1)->x + (tmp_all_points.end() - 1)->x - (tmp_all_points.end() - 2)->x;
                //     auto next_y = beta[2] * next_x * next_x + beta[1] * next_x + beta[0];
                //     this->x = next_x;
                //     this->y = next_y;
                // }
            }

            // if (new_points.cluster_points.size() > pre_obj)
            // {
            // this->length_box = new_points.length;
            // this->width_box = new_points.width;
            // this->psi = new_points.heading;
            // this->type = int(new_points.type);
            // this->height_box = new_points.height;
            // pre_obj = new_points;
            // }
            // else
            // {
            //     this->length_box = pre_obj.length;
            //     this->width_box = pre_obj.width;
            //     // this->psi = pre_obj.heading;
            //     this->type = int(pre_obj.type);
            //     this->height_box = pre_obj.height;

            // }

            cp = new_points.corner_point;

            std::cout << "id:" << id
                      << " x:" << this->x
                      << " y:" << this->y
                      << " this->cvx:" << this->cvx
                      << " this->cvy:" << this->cvy
                      << " heading:" << this->psi
                      << " type:" << this->type
                      << " confidence:" << this->confidence
                      << std::endl;
        }
        void MatchCenter::update(const double &time, const Localization &ego_pose)
        {
            // pre_heading = this->psi;
            this->age--;
            this->psi = pre_heading;
            if (this->cvx * this->cvx + this->cvy * this->cvy < 2 * 2)
                this->confidence -= this->dconfidence * 2;
            else
                this->confidence = this->confidence - this->dconfidence;
            this->x = this->cvx * (time - last_time) + this->x;
            this->y = this->cvy * (time - last_time) + this->y;
            last_time = time;
            std::cout << "up2 id:" << id
                      << " x:" << this->x
                      << " y:" << this->y
                      << " this->cvx:" << this->cvx
                      << " this->cvy:" << this->cvy
                      << " heading:" << this->psi
                      << " confidence:" << this->confidence
                      << std::endl;
        }
        bool MatchCenter::CheckStatic()
        {
            if (all_points.size() > 3)
            {
                float detx = std::fabs(sum_x / all_points.size() - this->x);
                float dety = std::fabs(sum_y / all_points.size() - this->y);
                // ERROR("detx:{},dety:{},sum_x:{},sum_y:{},size:{}", detx, dety, sum_x,
                // sum_y, all_points.size());
                if (detx < 2 && dety < 2)
                {
                    this->cvx = 0;
                    this->cvy = 0;
                    this->ax = 0;
                    this->ay = 0;
                    this->cx = detx;
                    this->cy = dety;
                    return true;
                }
            }

            return false;
        }
        bool MatchCenter::EsShape(LidarObject &new_points)
        {
            if (points_num < new_points.cluster_points.points.size())
            {
                float dl = new_points.length - this->length_box;
                float dw = new_points.width - this->width_box;
                if (dl < 0)
                    dl = 0;
                if (dw < 0)
                    dw = 0;
            }
        }
        bool MatchCenter::GetObj(LidarObject &new_points)
        {
            new_points.x = this->x;
            new_points.y = this->y;
            new_points.vx = this->cvx;
            new_points.vy = this->cvy;
            new_points.length = this->length_box;
            new_points.width = this->width_box;
            new_points.height = this->height_box;
            new_points.id = this->id;
            new_points.age = this->age;
            new_points.confidence = this->confidence;
            new_points.heading = this->psi;
            // new_points.type = this->type;
        }
    } // namespace algorithm
} // namespace perception